package edu.centenary.centenaryController;
import java.io.*;
import java.util.*;
/**
* Writes out messages sent by the server to a text file, writes the yaw values given by the server and by the robot's sensor when debugging, and turns the messages into information usable by the AI.
*/
public class SensorGather extends SleeperThread implements Runnable {
	 /**
	 * Sensor Data method object
	 */
    private SensorData sd;
	 /**
	 * Buffered Reader used to get text from server
	 */
	 
    private BufferedReader in;
	 /**
	 * The text sent by the server
	 */
    private String sensorInfo = "";
	 /**
	 * Checks to see if this is the first time through the while loop.  Ensures that existing output file is overwritten at the start of each run.
	 */
    private boolean firstTime = true;
	 /**
	 * Print Writer that outputs server messages to a file
	 */
    private PrintWriter fileOut = null;
	 /**
	 * The name of the file and file path of the saved output
	 */
    private String filePath = "serveroutput.txt";
	 /**
	 * For testing purposes! Outputs the INS sensor's yaw and the robot's real yaw.  Yes, they are different.
	 */
	 //private PrintWriter yawOut = null;

	 /**
	 * Constructor method for SensorGather
	 * @param sd SensorData object
	 * @param in Buffered Reader that gets text from the server
	 */
    public SensorGather(SensorData sd, BufferedReader in, String name) {
        this.sd = sd;
        this.in = in;
		  this.filePath = "serveroutput" + name +".txt";
    }

	 /**
	 * The main method of the SensorGather thread.  Outputs server messages to a file and extracts valuable informaton from Sonar, RangeScanner, INS, and Victim sensors.
	 */
    public void run() {
        while (!Thread.currentThread().isInterrupted()) {
            try {
					//yawOut = new PrintWriter(new FileWriter("YawTest.txt", true));
                //Gets server info
                while (in.ready() && !Thread.currentThread().isInterrupted()) {
                    sensorInfo = in.readLine();
                    if (firstTime == true) {
						  		
                        fileOut = new PrintWriter(new FileWriter(filePath, false));
                        firstTime = false;
                    } else {
                        fileOut = new PrintWriter(new FileWriter(filePath, true));
								
                    }
                    fileOut.println(sensorInfo);
                    fileOut.flush();
                    fileOut.close();
						  
                    StringTokenizer st = new StringTokenizer(sensorInfo," ,{}");
                    String type = st.nextToken();
                    if (type.equals("NFO")) {

                        //Gets system output only for GETSTARTPOSES
                        if (st.nextToken().equals("StartPoses") && !(sd.numberOfPoses() > 1)) {
                            st.nextToken();
                            int i = 0;
                            while (st.hasMoreTokens()) {

                                //A pose includes an identifying #, x, y, x, roll, pitch, & yaw values
                                st.nextToken();
                                Pose p = new Pose(i, Double.parseDouble(st.nextToken()), Double.parseDouble(st.nextToken()), Double.parseDouble(st.nextToken()), Double.parseDouble(st.nextToken()), Double.parseDouble(st.nextToken()), Double.parseDouble(st.nextToken()));
                                sd.addPose(p);
                                i++;
                                //System.out.println("Added Pose" + i);
                            }

                        }

                    } else if (type.equals("SEN")) {
                        st.nextToken();
                        st.nextToken();
                        st.nextToken();
                        String senType = st.nextToken();
                        if (senType.equals("Sonar")) {
                            while (st.hasMoreTokens()) {
                                st.nextToken();
                                String name = st.nextToken();
                                st.nextToken();
                                String value = st.nextToken();
                                //		System.out.println("Sonar Data for" + name + ":" + value);
                                sd.setSonar(name, new Double(value));
                            }
                        } else if (senType.equals("RangeScanner")) {
                            int i = 0;
                            st.nextToken();
                            st.nextToken();
                            st.nextToken();
                            st.nextToken();
                            st.nextToken();
                            st.nextToken();
                            st.nextToken();
                            while (st.hasMoreTokens()) {
                                double range = Double.parseDouble(st.nextToken());
                                sd.setRangeScanner(i, range);
                                //System.out.println("Range Sensor Data: \n" + sd.getRangeScanner(i) +":  " + i);
                                i++;
                            }
								} else if (senType.equals("Odometry")) {
                            st.nextToken();
                            
                            double odLocationX = Double.parseDouble(st.nextToken());
                            double odLocationY = Double.parseDouble(st.nextToken());
                            double odTheta = Double.parseDouble(st.nextToken());
									 //System.out.println("odLocationX is " + odLocationX);
									 //System.out.println("odLocationY is " + odLocationY);
									 //System.out.println("odTheta is " + odTheta);
                            sd.setOdLocationX(odLocationX);
									 sd.setOdLocationY(odLocationY);
									 sd.setOdTheta(odTheta);
							/*	} else if (senType.equals("Encoder")) {
                            st.nextToken();
                            st.nextToken();
									 st.nextToken();
									 int ecLeftTick = Integer.parseInt(st.nextToken());
									 st.nextToken();
                            st.nextToken();
									 st.nextToken();
									 int ecRightTick = Integer.parseInt(st.nextToken());
									 System.out.println("ECLeft Tick is " + ecLeftTick);
									 System.out.println("ECRight Tick is " + ecRightTick);
									 
                            sd.setOdLocationX(odLocationX);
									 sd.setOdLocationY(odLocationY);
									 sd.setOdLocationZ(odLocationZ);
									 */
                        } else if (senType.equals("Touch")) {
                            st.nextToken();
                            boolean touch = Boolean.parseBoolean(st.nextToken());
                            //	System.out.println("Parsed touch boolean");
                            sd.setTouch(touch);
                        } else if (senType.equals("INS")) {
									//System.out.println("INS output: " + sensorInfo);
                           /* st.nextToken();
                            double locationX = Double.parseDouble(st.nextToken());
                            double locationY = Double.parseDouble(st.nextToken());
                            double locationZ = Double.parseDouble(st.nextToken());
                            st.nextToken();
                            double roll = Double.parseDouble(st.nextToken()) %6.28;
                            double pitch = Double.parseDouble(st.nextToken()) %6.28;
                            double yaw = Double.parseDouble(st.nextToken()) %6.28;
									 //yawOut.print(yaw);
									 //yawOut.flush();
                            sd.setLocationX(locationX);
                            sd.setLocationY(locationY);
                            sd.setLocationZ(locationZ);
                            sd.setRoll(roll);
                            sd.setPitch(pitch);
                            sd.setYaw(yaw);
									 */
                        }else if (senType.equals("GroundTruth")) {
									//System.out.println("Server Truth: " + sensorInfo);
                            st.nextToken();
									 st.nextToken();
									 st.nextToken();
                            double locationX = Double.parseDouble(st.nextToken());
                            double locationY = Double.parseDouble(st.nextToken());
                            double locationZ = Double.parseDouble(st.nextToken());
                            st.nextToken();
                            double roll = Double.parseDouble(st.nextToken()) %6.28;
                            double pitch = Double.parseDouble(st.nextToken()) %6.28;
                            double yaw = Double.parseDouble(st.nextToken()) %6.28;
                            sd.setLocationX(locationX);
                            sd.setLocationY(locationY);
                            sd.setLocationZ(locationZ);
                            sd.setRoll(roll);
                            sd.setPitch(pitch);
                            sd.setYaw(yaw); 
									 
								}else if (senType.equals("VictSensor")) {
									 sd.resetParts();
                            if (st.nextToken().equals("Name")) {
                                st.nextToken();
                                while (st.hasMoreTokens()) {
                                    st.nextToken();

                                    String bodyPart = st.nextToken();
                                    //System.out.println(bodyPart);
                                    st.nextToken();
                                    double bodyPartLocationX = Double.parseDouble(st.nextToken());
                                    //System.out.println(bodyPartLocationX);
                                    double bodyPartLocationY = Double.parseDouble(st.nextToken());
                                    //System.out.println(bodyPartLocationY);
                                    double bodyPartLocationZ = Double.parseDouble(st.nextToken());
                                    //System.out.println(bodyPartLocationZ);
                                    sd.setVictSense(bodyPart, bodyPartLocationX, bodyPartLocationY, bodyPartLocationZ);
                                }
                            } else {
                                //Do nothing
                            }
                        } else {

                        }


                    }
                }
				//yawOut.flush();
            //yawOut.close();
            } catch (IOException e) {
                System.out.println("Problem getting readLine in AI");
            }
            //sleep(50);

        }
    }
}



